#include "Handler_Command.h"
#include <stddef.h>
#include <string.h>
#include "Motor.h"
#include "ServoMotor.h"
#include "Buzzer.h"

static CommandExecutor_Type _MotorDirection = CMD_GFW;
static CommandExecutor_Type _MotorSpeed = CMD_SD0;

// 指令字符串表
static const char* const command_strings[] = {
  "@GFW#",   // 前进
  "@GBW#",   // 后退
  "@TL#",    // 左转
  "@TR#",    // 右转 
  "@TS#",    // 直行
  "@SD1#",   // 挡位1
  "@SD2#",   // 挡位2
  "@SD3#",   // 挡位3 
  "@SD0#",   // 空挡
  "@SDB#",   // 刹车
  "@BZ0#",    // 蜂鸣器停止
  "@BZ1#",    // 蜂鸣器频率1
  "@BZ2#",    // 蜂鸣器频率2
  "@BZ3#",    // 蜂鸣器频率3
};

#define COMMAND_LENGTH 16

// 查找指令并返回枚举值
CommandExecutor_Type CommandExecutor_LookupCommand(const char* cmd) {
  size_t cmd_len;
  uint8_t i;
  if (cmd == NULL) {
    return CMD_INVALID;  // 空指针检查
  }
    
  // 根据长度区分指令类型
  cmd_len = strlen(cmd);

  for (i = 0; i < COMMAND_LENGTH; i++) {
    if (strncmp(cmd, command_strings[i], cmd_len) == 0 &&
      strlen(command_strings[i]) == cmd_len) {
      return (CommandExecutor_Type)i;
    }
  }
  return CMD_INVALID;
}

void _MotorRun() {
  if (_MotorSpeed == CMD_SD1) {
    Motor_Control(_MotorDirection == CMD_GFW ? MotorDirection_Forward : MotorDirection_Backward, 20);
  }
  else if (_MotorSpeed == CMD_SD2) {
    Motor_Control(_MotorDirection == CMD_GFW ? MotorDirection_Forward : MotorDirection_Backward, 40);
  }
  else if (_MotorSpeed == CMD_SD3) {
    Motor_Control(_MotorDirection == CMD_GFW ? MotorDirection_Forward : MotorDirection_Backward, 60);
  }
  else if (_MotorSpeed == CMD_SD0) {
    Motor_Control(_MotorDirection == CMD_GFW ? MotorDirection_Forward : MotorDirection_Backward, 0);
  }
  else if (_MotorSpeed == CMD_SDB) {
    Motor_Break();
  }
}

// 处理指令
uint8_t CommandExecutor_ProcessCommand(CommandExecutor_Type cmd) {
  if (cmd == CMD_INVALID) {
    return 0;  // 无效指令
  }
  switch(cmd) {
    case CMD_GFW:
    case CMD_GBW: {
      if (_MotorDirection != cmd) {
        Motor_Break();
        _MotorDirection = cmd;
        LL_mDelay(500);
      }
      _MotorRun();
      return 1;
    }
    case CMD_TL:
      ServoMotor_TurnLeft();
      return 1;
    case CMD_TR:
      ServoMotor_TurnRight();
      return 1;
    case CMD_TS:
      ServoMotor_TurnStright();
      return 1;
    case CMD_SD1:
    case CMD_SD2:
    case CMD_SD3:
    case CMD_SD0:
    case CMD_SDB: {
      _MotorSpeed = cmd;
      _MotorRun();
      return 1;
    }
    case CMD_BZ0:
      Buzzer_bee(Buzzer_Type_Bee_stop);
      return 1;
    case CMD_BZ1:
      Buzzer_bee(Buzzer_Type_Bee1);
      return 1;
    case CMD_BZ2:
      Buzzer_bee(Buzzer_Type_Bee2);
      return 1;
    case CMD_BZ3:
      Buzzer_bee(Buzzer_Type_Bee3);
      return 1;
    default:
      return 0;
  }
}
